#ifndef __CONTROLLER_ROS_NODE_H__
#define __CONTROLLER_ROS_NODE_H__

#include <ros/ros.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> 
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <tr_msgs/ControllerCmd.h>

#include <mc_ros/visualization/controller_visualization.h>
#include <mc_ros/processor/state_machine.h>
#include <mc_ros/processor/processor_p2p.h>
#include "core/debug/tracked_path.h"
#include "core/robot_model/robot_model_base.h"
#include "perception/costmap_2d/costmap_2d_ros.h"
#include "path_generate/p2p/astar.h"
#include "path_generate/p2p/jps.h"
#include "controller/dwa/dwa.h"
#include "controller/pure_pursuit/pure_pursuit.h"
#include "controller/stanley/stanley.h"
#include "controller/lqr/lqr.h"

using namespace std;

class ControllerRosNode{
    public:
        ControllerRosNode(tf2_ros::Buffer* tf_buffer);
        ~ControllerRosNode();

         // 循环控制
        void Loop();

    private:
        // 地图回调函数
        void MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg);
        // 二维雷达回调函数
        void LidarCallback(const sensor_msgs::LaserScan::Ptr& msg);
        // Rviz位姿回调函数
        void RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg);
        // 融合数据回调函数
        void FusionDataCallback(const sensor_msgs::PointCloud2::Ptr& msg);
        // 里程计回调函数
        void OdometryCallback(const nav_msgs::Odometry::Ptr& msg);
        // 全局代价地图回调函数
        void CostmapGlobalCallback(const nav_msgs::OccupancyGrid::Ptr& msg);
        // 局部代价地图回调函数
        void CostmapLocalCallback(const nav_msgs::OccupancyGrid::Ptr& msg);
        

        // 服务回调函数
        bool ControllerCmdCallback(tr_msgs::ControllerCmd::Request& req, tr_msgs::ControllerCmd::Response& res);

    private:
        ros::NodeHandle nh_;
        tf2_ros::Buffer* const tfBuffer_;
        ros::Subscriber rvizPoseSub_;
        ros::Subscriber mapSub_;
        ros::Subscriber costmapGlobalSub_;
        ros::Subscriber costmapLocalSub_;
        // ros::Subscriber lidarScanSub_;
        ros::Subscriber fusionDataSub_;
        ros::Subscriber odomSub_;

        ros::Publisher mapBevPub_;
        ros::Publisher mapDilatePub_;
        // ros::Publisher trackedPathPub_;         //
        // ros::Publisher robotModelMarkerPub_;    //机器人模型发布者
        ros::Publisher costmapLocalPub_;
        ros::Publisher costmapGlobalPub_;

        ros::ServiceServer controllerCmdService_;

        nav_msgs::OccupancyGrid::Ptr occupancyGrid_;    // 地图数据
        boost::shared_ptr<const geometry_msgs::PoseStamped> goalPosePtr_;   //Rviz位姿
        boost::shared_ptr<sensor_msgs::PointCloud2> fusionDataPtr_;         //传感器融合数据
        boost::shared_ptr<nav_msgs::Odometry> odomDataPtr_;    
        boost::shared_ptr<nav_msgs::OccupancyGrid> costmap_local_ptr_;     
        boost::shared_ptr<nav_msgs::OccupancyGrid> costmap_global_ptr_; 

        // boost::shared_ptr<costmap_2d::Costmap2DROS> costmapLocalPtr_;
        // boost::shared_ptr<costmap_2d::Costmap2DROS> costmapGlobalPtr_;

        // boost::shared_ptr<MapBase> mapHandlerPtr_;
        boost::shared_ptr<PlannerBase> globalPathPlannerPtr_;          //路径规划器
        boost::shared_ptr<ControllerBase> localControllerPtr_;        //速度规划器
        boost::shared_ptr<StateMachineBase> processorP2P_;          //核心控制器
        boost::shared_ptr<ModelBase> robotModelPtr_;        //机器人模型

        // TrackedPath trackedPath_;       //路径跟踪
        string robotModelConfigFile_;   //机器人模型配置文件

        boost::shared_ptr<MCVisualization> mcVisualizationPtr_;

};

#endif